#! /usr/bin/python3

# import os
# import sys
# import argparse
# import subprocess
# import rospy
# import tf2_ros
# import tf
# from geometry_msgs.msg import TransformStamped


# if __name__ == "__main__":
#     rospy.init_node("SCTransfor_Pub")
#     print('---------start--------------')
#     broadcaster = tf2_ros.StaticTransformBroadcaster()
#     tfs=TransformStamped()
#     tfs.header.frame_id="world"
#     tfs.header.stamp=rospy.Time.now()
#     tfs.header.seq=101

#     tfs.child_frame_id="laser"
#     tfs.transform.translation.x=0.2
#     tfs.transform.translation.y=0.0
#     tfs.transform.translation.z=0.5

#     qtn=tf.transformations.quaternion_from_euler(0,0,0)
#     tfs.transform.rotation.x=qtn[0]
#     tfs.transform.rotation.y=qtn[1]
#     tfs.transform.rotation.z=qtn[2]
#     tfs.transform.rotation.w=qtn[3]

#     broadcaster.sendTransform(tfs)
#     rospy.spin()
# 1.导包
import rospy
import tf2_ros
import tf
from geometry_msgs.msg import TransformStamped

if __name__ == "__main__":
    # 2.初始化 ROS 节点
    rospy.init_node("SCTransfor_Pub")
    # 3.创建 静态坐标广播器
    broadcaster = tf2_ros.StaticTransformBroadcaster()
    # 4.创建并组织被广播的消息
    tfs = TransformStamped()
    # --- 头信息
    tfs.header.frame_id = "world"
    tfs.header.stamp = rospy.Time.now()
    tfs.header.seq = 101
    # --- 子坐标系
    tfs.child_frame_id = "radar"
    # --- 坐标系相对信息
    # ------ 偏移量
    tfs.transform.translation.x = 0.2
    tfs.transform.translation.y = 0.0
    tfs.transform.translation.z = 0.5
    # ------ 四元数
    qtn = tf.transformations.quaternion_from_euler(0,0,0)
    tfs.transform.rotation.x = qtn[0]
    tfs.transform.rotation.y = qtn[1]
    tfs.transform.rotation.z = qtn[2]
    tfs.transform.rotation.w = qtn[3]


    # 5.广播器发送消息

    broadcaster.sendTransform(tfs)
    rospy.loginfo("Transform from 'world' to 'radar' broadcasted.")
    rospy.spin() 




